/**
 *  @author: Kris Bechamp
 *  @date:   2011.10.17
 *
 *  @brief:
 *      Simple wall hugging robot
 */
#include "launchpad.h"
#include "mctrl.h"

// Function Declarations (Prototypes)
void init(void);
void DriveRoutine(void);

// Global Variables
enum DRIVESTATE {STOPPED, MOVING};
enum DRIVESTATE driveState;


int main(void) {

    init();

    BITSET(P1OUT, LED_R);
    BITCLR(P1OUT, LED_G);
    MCTRL_Stop();
    driveState = STOPPED;

    while (1){
        (driveState == STOPPED) ? MCTRL_Stop() : DriveRoutine();
    }

    return 0;
}

/**
 * Initialization function
 */
void init(void) {
    LAUNCHPAD_Init();
    MCTRL_Init();
}

void DriveRoutine(void) {
    if (driveState)     MCTRL_DriveForward();   delay_ms(2000);
    if (driveState)     MCTRL_Stop();           delay_ms(500);
    if (driveState)     MCTRL_SpinRight();      delay_ms(1000);
    if (driveState)     MCTRL_Stop();           delay_ms(500);
}

/**
 * Port 1 interrupt service routine
 */
void __attribute__((interrupt (PORT1_VECTOR))) PORT1_ISR(void) {

    P1IFG &= ~USR_BUTTON;                         // P1.3 IFG cleared

    if (driveState == MOVING) {
        MCTRL_Stop();
        driveState = STOPPED;
        BITSET(P1OUT, LED_R);
        BITCLR(P1OUT, LED_G);
//        _BIS_SR(LPM4_bits + GIE); // Enter LPM4 w/interrupt
    } else {
        BITSET(P1OUT, LED_G);
        BITCLR(P1OUT, LED_R);
        driveState = MOVING;
    }
}
